//
// Created by jg on 2020/9/8.
//
#include <ros/ros.h>
#include <fstream>

#include "oriTrans.h"
#include "loc2pos.h"
#include "gpstime.h"

#include "waypoint_msgs.h"

#include "path_msgs/PathMsg.h"
#include "wmr_msgs/PolynomialTrajectory.h"

//std::ofstream wf_;

//#define D2R M_PI/180
//#define R2D 180/M_PI
//#define RA  6378137.0                  //长半轴
//#define E2  0.00669437999013           //偏心率平方

std::string double2string(const double value) {
    std::ostringstream out;
    out.precision(15);
    out << value;
    return out.str();
}

typedef struct {
    double   unixtime;
    uint16_t week;    /* GPS周 */
    double   weeksec; /* GPS周内秒,s */

    double lat; /* 纬度,deg */
    double lon; /* 经度,deg */
    double alt; /* 椭球高,m */

    double velnorth; /* 北向速度,m/s */
    double veleast;  /* 东向速度,m/s */
    double veldown;  /* 地向速度,m/s */

    double roll;  /* 横滚角,deg */
    double pitch; /* 俯仰角,deg */
    double yaw;   /* 航向角,deg */

    uint8_t navmode; /* 导航模式, 0-纯惯导, 1-SPP松组合, 2-FLOAT松组合, 3-FIXED松组合 */
//    uint8_t flag;    /* 0-END, 1-WITH IMU, 2-WITH GNSS */
//
//    float gyrox;  /* 陀螺X */
//    float gyroy;  /* 陀螺Y */
//    float gyroz;  /* 陀螺Z */
//    float accelx; /* 加表X */
//    float accely; /* 加表Y */
//    float accelz; /* 加表Z */
//
//    double gnsslat;    /* GNSS纬度 */
//    double gnsslon;    /* GNSS经度 */
//    float  gnssalt;    /* GNSS高程 */
//    float  gnsslatstd; /* GNSS纬度标准差 */
//    float  gnsslonstd; /* GNSS经度标准差 */
//    float  gnssaltstd; /* GNSS经度标准差 */
//
//    uint8_t check1;
//    uint8_t check2;
} InsprobePack;

InsprobePack insprobe_pack_;
std::mutex msg_lock_;
ros::Publisher pos_pub_, target_pub_, path_pub_;

bool hasPos_ = false;
std::string map_file_;

//map information
double resolution, x_min_, x_max_, y_min_, y_max_;
//map's original point   lat -deg;  lon -deg;   alt  -m;
Eigen::Vector3d original_point;
bool has_original_ = false, has_ins_msgs_ = false;
//location translation
LOC2POS* pos_trans_ = new LOC2POS();

//receive the location information -- insprobe_msgs
void locationCallback(insprobe_msgs::Navigation msg){
    msg_lock_.lock();

//    wf_ << "[insprobe pos] insprobe TIME " << double2string(msg.header.stamp.toSec())
//    << ", ROS TIME " << double2string(ros::Time::now().toSec()) << std::endl;

    insprobe_pack_.unixtime = msg.header.stamp.toSec();

    insprobe_pack_.week     = msg.week;
    insprobe_pack_.weeksec  = msg.weeksec;
    insprobe_pack_.lat      = msg.latitude;        // deg
    insprobe_pack_.lon      = msg.longitude;       // deg
    insprobe_pack_.alt      = msg.altitude;        // m
    insprobe_pack_.pitch    = msg.pitch;           // deg
    insprobe_pack_.roll     = msg.roll;            // deg
    insprobe_pack_.yaw      = msg.yaw;             // deg
    insprobe_pack_.veldown  = msg.velocity_ground;
    insprobe_pack_.veleast  = msg.velocity_east;
    insprobe_pack_.velnorth = msg.velocity_north;
    insprobe_pack_.navmode  = msg.nav_state;       // int

    insprobe_pack_.yaw     *= D2R;
    insprobe_pack_.roll    *= D2R;
    insprobe_pack_.pitch   *= D2R;

    insprobe_pack_.lat     *= D2R;
    insprobe_pack_.lon     *= D2R;

    msg_lock_.unlock();

    has_ins_msgs_ = true;
}
//initiate the map information: resolution, map size, original point.
void readMap(){
    std::ifstream fp;
    fp.open(map_file_);
    if(!fp){
        std::cerr << "[insprobe_to_pos]:open map file error !" << std::endl;
        exit(-1);
    }

    std::string str;
    while(getline(fp, str)){
        if(str.empty())
            std::cout << "nod data !!!!!!" << std::endl;
        if (str[0] == '$'){
            int index = str.find_first_of(',');
            std::string head = str.substr(0, index);
            if (head == "$resolution"){
                resolution = atof(str.substr(index+1, str.size()-index-1).c_str());
            }
            else if(head == "$original_point"){
                str = str.substr(index+1, str.size()-index-1);
                index = str.find(',');

                double lat = atof(str.substr(0, index).c_str());       //deg
                str = str.substr(index+1, str.size()-index-1);
                index = str.find(',');

                double lon = atof(str.substr(0, index).c_str());       //deg
                str = str.substr(index+1, str.size()-index-1);

                double alt = atof(str.c_str());                                 //m
                original_point << lat*D2R, lon*D2R, alt;                        //rad, rad, m
                has_original_ = true;
            }
            else if(head == "$mapsize"){
                str = str.substr(index+1, str.size()-index-1);
                index = str.find(',');

                x_min_ = atof(str.substr(0, index).c_str());
                str = str.substr(index+1, str.size()-index-1);
                index = str.find(',');

                x_max_ = atof(str.substr(0, index).c_str());
                str = str.substr(index+1, str.size()-index-1);
                index = str.find(',');

                y_min_ = atof(str.substr(0, index).c_str());
                str = str.substr(index+1, str.size()-index-1);

                y_max_ = atof(str.c_str());
            }
        }
    }
    //initiate the pos_trans_
    pos_trans_ = new LOC2POS(original_point, resolution);

    fp.close();
}

//publish nav_msgs pos
oriTrans orientationTrans_;
nav_msgs::Odometry state_;
Eigen::Quaterniond quan_robot_;

double yaw_last_, robot_time_last_;
void posPublish(const ros::TimerEvent& event){
    if(!has_ins_msgs_) return;

//    ros::Time t1 = ros::Time::now();
    state_.header.frame_id  = "wmr";
//    double unix_time;
//    GpsTime::gpsSecond2UnixSecond(unix_time, insprobe_pack_.week, insprobe_pack_.weeksec);
    state_.header.stamp = ros::Time(insprobe_pack_.unixtime);
//    ROS_INFO("[pos-local]: %lf", state_.header.stamp.toSec());

    //Convert latitude and longitude height(BHL -deg -deg -m) to local coordinate system(xyz -m)
    if(!has_original_ || !has_ins_msgs_) return;
    msg_lock_.lock();
    double vn = insprobe_pack_.velnorth;
    double ve = insprobe_pack_.veleast;
//    double vd = insprobe_pack_.veldown;
    Eigen::Vector3d loc_BLH(insprobe_pack_.lat, insprobe_pack_.lon, insprobe_pack_.alt);

    //偏航角yaw为北偏东为正，转换到局部坐标系时需要作处理
    //地图x轴正指向北、y轴正指向西
    double yaw = -insprobe_pack_.yaw;
//    yaw = (double)M_PI/2 - yaw;
    while(yaw > 2*M_PI) yaw -= 2*M_PI;
    while(yaw < 0) yaw += 2*M_PI;
    Eigen::Vector3d Euler(insprobe_pack_.roll, insprobe_pack_.pitch, yaw);

    static bool isFirst  = true;
    static bool overMark = true;
    static short _count  = 5;
    if(isFirst && (!overMark && insprobe_pack_.weeksec != robot_time_last_)){
        yaw_last_ = yaw;
        isFirst   = false;
        overMark  = true;
        _count    = 5;
        robot_time_last_ = insprobe_pack_.weeksec;
    } else if(insprobe_pack_.weeksec == robot_time_last_){

        if(overMark){
            ROS_WARN("[insprobe-to-pos node]: weeksec is over ");
            overMark = false;
        }

        if(_count == 0){
            isFirst = true;
            _count--;
            ROS_WARN("[insprobe-to-pos node]: no insprobe message !");
        }
        else if(_count > 0)
            _count--;

        msg_lock_.unlock();
        return;
    } else {
        if(!overMark){
            overMark = true;
            _count = 5;
        }
    }

    Eigen::Vector2d loc_xy;
    pos_trans_->BLH2xy(loc_BLH, loc_xy);
//    std::cout << "loc xy : " << loc_xy[0] << ", " << loc_xy[1] << std::endl;

    state_.pose.pose.position.x = loc_xy[0];
    state_.pose.pose.position.y = loc_xy[1];
    state_.pose.pose.position.z = 0;

    orientationTrans_.Euler2Quater(Euler, &quan_robot_);
    state_.pose.pose.orientation.w = quan_robot_.w();
    state_.pose.pose.orientation.x = quan_robot_.x();
    state_.pose.pose.orientation.y = quan_robot_.y();
    state_.pose.pose.orientation.z = quan_robot_.z();

    state_.twist.twist.linear.x = vn;
    state_.twist.twist.linear.y = -ve;
    state_.twist.twist.linear.z = 0;   //将该项设置为0, 若赋予地速值，则可能出现由于存在z轴速度而无法规划轨迹

    //向左转角速度为正，向右为负
    double d_yaw   = yaw - yaw_last_;
    if (d_yaw > M_PI) {
        yaw_last_ += 2*M_PI;
        d_yaw      = yaw - yaw_last_;
    } else if (d_yaw < -M_PI) {
        yaw_last_ -= 2*M_PI;
        d_yaw      = yaw - yaw_last_;
    }

    state_.twist.twist.angular.x = 0;
    state_.twist.twist.angular.y = 0;
    state_.twist.twist.angular.z = d_yaw / (insprobe_pack_.weeksec - robot_time_last_);

    yaw_last_        = yaw;
    robot_time_last_ = insprobe_pack_.weeksec;
    msg_lock_.unlock();

//    wf_ << "[insprobe pos] pos TIME " << double2string(state_.header.stamp.toSec())
//        << ", ROS TIME " << double2string(ros::Time::now().toSec()) << std::endl;
    pos_pub_.publish(state_);
    ROS_INFO("[pos-local]: %lf, %0.3f, %0.3f, %0.3f", state_.header.stamp.toSec(),
             state_.pose.pose.position.x, state_.pose.pose.position.y, yaw*180/M_PI);
//    ros::Time t2 = ros::Time::now();
//    ROS_INFO("[pos generator]: generator time is %lf", t2.toSec() - t1.toSec());
    if(!hasPos_) hasPos_ = true;
}

void targetCallback(geometry_msgs::Pose2D msg){
    if(!hasPos_) return;
    //msg -deg, deg, m
    double target_lat = msg.x;
    double target_lon = msg.y;
    double ctrl_mode  = msg.theta;  //0-handle; 1 -auto; 2 -loop

    Eigen::Vector3d target_BLH(target_lat*D2R, target_lon*D2R, original_point[2]);
    Eigen::Vector2d target_xy;
    pos_trans_->BLH2xy(target_BLH, target_xy);
//    auto target_xy = pos_trans_->BLH2xy(target_BLH);

    geometry_msgs::PoseStamped target_point;
    nav_msgs::Path pt_target;
    static int seq_pt = 0;
    if(ctrl_mode == HANDLE || ctrl_mode == AUTO){
        pt_target.header.stamp.sec = insprobe_pack_.weeksec;
        pt_target.header.frame_id  = (ctrl_mode == HANDLE) ?
                std::string("HANDLE") : std::string("AUTO");

        target_point.header.stamp.sec = insprobe_pack_.weeksec;
        target_point.header.frame_id  = std::string("odom");
        if(ctrl_mode == HANDLE) seq_pt = 0;
        target_point.header.seq = seq_pt++;

        target_point.pose.position.x  = target_xy[0];
        target_point.pose.position.y  = target_xy[1];
        target_point.pose.position.z  = 0.1;

        pt_target.poses.push_back(target_point);
    }
    else if (ctrl_mode == LOOP){
        //
    }

    target_pub_.publish(pt_target);
}

void bezierTrajCallback(wmr_msgs::PolynomialTrajectory msg){
    path_msgs::PathMsg path;
    path.mode = 0;

//    for(int i = 0; i < msg.num_segment; i++){
//        for(int j = 0; j < msg.order+1; j++){
//            Eigen::Vector2d xy(msg.coef_x[i] , msg.coef_y[i]);
//        }
//    }

    int time_index = 0;
    for(int i = 0; i < (int)msg.coef_x.size(); i++){
        time_index = i / (int)(msg.order.front()+1);
        Eigen::Vector2d xy(msg.coef_x[i] * msg.time[time_index] , msg.coef_y[i] * msg.time[time_index]);
        Eigen::Vector3d BLH;
        pos_trans_->XY2BLH(xy, BLH);
        path.lat.push_back(BLH[0]);
        path.lon.push_back(BLH[1]);
    }

    path_pub_.publish(path);
}

void exitMain(){
//    wf_.close();
    delete pos_trans_;
}

int main(int argc, char **argv){
    ros::init(argc, argv, "insprobe_to_pos");
    ros::NodeHandle n("~");

    n.param<std::string>("map/path", map_file_,
    std::string("/home/jg/catkin_ws/btraj/wmr_bezier_planner/map/information_department_index.map"));

    ros::Subscriber location_sub =
            n.subscribe<insprobe_msgs::Navigation>("/insprobe/nav0", 1, locationCallback);
    ros::Subscriber targetpoint_sub =
            n.subscribe<geometry_msgs::Pose2D>("/target_pose", 10, targetCallback);
    ros::Subscriber trajectory_sub =
            n.subscribe<wmr_msgs::PolynomialTrajectory>("/trajectory", 10, bezierTrajCallback);

    pos_pub_    = n.advertise<nav_msgs::Odometry>("/pos",     1);
    target_pub_ = n.advertise<nav_msgs::Path>("/waypoints",   10);
    path_pub_   = n.advertise<path_msgs::PathMsg>("/path_vec",10);

//    wf_.open("/home/jg/catkin_ws/src/wmr_bezier_planner/record_time.txt");
//    if(!wf_){
//        std::cerr << "[insprobe_to_pos]:open map file error !" << std::endl;
//        exit(-1);
//    }

    readMap();

    ros::Timer timer = n.createTimer(ros::Duration(0.02), posPublish);
    ros::spin();

    atexit(exitMain);
    return 0;
}